1. Field of the Invention
The invention relates to planning and controlling the motion of an object and in particular to path planning in a highway environment.
2. Related Art
The field of path planning is one with many applications. In the prior art, the most common application is for controlling robots. Other applications include electronic maps, traffic control, emergency vehicle control, emergency exit systems and vehicle maneuvering.
The path planning problem, as applied to robots, typically involves choosing a path to get a robot from a start point to a goal point while avoiding obstacles. Automatic multidimensional path planning for robots is one of the great historical problems of robotics.
The disclosure of U.S. Pat. No. 4,949,277 is incorporated herein by reference. The prior art discloses, amongst other things, propagating cost waves through a configuration space by budding, using a space-variant metric.
Budding finds the optimal path from all starting states to the nearest goal state in a configuration space. A configuration state can be described by a tuple. A tuple is a list of valued parameters. The parameters are always given in a specific order so that an implicit correspondence can be made between the list of values and the associated parameters. For example, an x, y tuple is typically written as [2,4] or alternatively, (2,4). In this case, the value of `x` in the tuple is 2, and the value of `y` is 4. The configuration space is typically the span of tuples. Tuple, also defines as a `setpoint`. For example, the (2,4) tuple may represent the offset measures from the origin of: 2 meters away in the `x` direction, and 4 meters away in the `y` direction. The configuration space is therefore a discretized range of values for each of the parameter in a device. A cost metric is imposed on the configuration space. This cost metric determines the transition cost of moving from one state to another state. In some cases, it may be `illegal` or highly undesirable for the device to be in a particular configuration state. These states are referred to as `obstacles` and have a very high, or infinite transition cost from any other state. A `neighborhood` describes the capabilities of the device. The neighborhood gives the permissible state changes from a given state. Budding starts at the goal or goals, and expands adjacent nodes least-total-cost-first. A graphical view of the order of expansion gives rise to the `wave propagation` term. By propagating waves cost-first the configuration space is also marked in the order of expansion with `direction arrows` which point back to the least cost path at each configuration state. From a starting state, the goal is reached by following the `direction arrows` from state to neighboring state until the goal state is reached. The goal state has no direction arrows. By reading out the tuples representing the state, the device can be controlled. This is typically performed by sending the `setpoints` to the device's controller.
After budding, some aspect of the configuration space may change, for example, if an obstacle or goal is added or removed. In such a case, it may be inefficient to bud the entire configuration space again because only a small part of the configuration space may be affected.
`Differential budding` has been extended to handle not only added or removed goals and obstacles, but also the individual transitions between states.
The `budding` and `differential budding` methods have been used to control the maneuvers of a robotic vehicle. In this case, the configuration space of the vehicle is described in terms of the position and orientation of the vehicle relative in a fixed world position. In this way, the path planner generates `setpoints` of position and orientation which the vehicle can then carry out.